//
// Source code recreated from a .class file by IntelliJ IDEA
// (powered by FernFlower decompiler)
//

package com.ruoyi.common.utils.jkalman.jkalman;


import com.ruoyi.common.utils.jkalman.jama.Matrix;

public class JKalman {
    public static double Q = 0.007;
    public static double R = 0.005;
    int mp;
    int dp;
    int cp;
    Matrix state_pre;
    Matrix state_post;
    Matrix transition_matrix;
    Matrix control_matrix;
    Matrix measurement_matrix;
    Matrix process_noise_cov;
    Matrix measurement_noise_cov;
    Matrix error_cov_pre;
    Matrix gain;
    Matrix error_cov_post;
    Matrix temp1;
    Matrix temp2;
    Matrix temp3;
    Matrix temp4;
    Matrix temp5;

    public JKalman(int dynam_params, int measure_params, int control_params) throws Exception {
        if (dynam_params > 0 && measure_params > 0) {
            if (control_params < 0) {
                control_params = dynam_params;
            }

            this.dp = dynam_params;
            this.mp = measure_params;
            this.cp = control_params;
            this.state_pre = new Matrix(this.dp, 1);
            this.state_post = new Matrix(this.dp, 1);
            this.transition_matrix = Matrix.identity(this.dp, this.dp);
            this.process_noise_cov = Matrix.identity(this.dp, this.dp, Q);
            this.measurement_matrix = Matrix.identity(this.mp, this.dp);
            this.measurement_noise_cov = Matrix.identity(this.mp, this.mp, R);
            this.error_cov_pre = new Matrix(this.dp, this.dp);
            this.error_cov_post = Matrix.identity(this.dp, this.dp);
            this.gain = new Matrix(this.dp, this.mp);
            if (this.cp > 0) {
                this.control_matrix = new Matrix(this.dp, this.cp);
            } else {
                this.control_matrix = null;
            }

            this.temp1 = new Matrix(this.dp, this.dp);
            this.temp2 = new Matrix(this.mp, this.dp);
            this.temp3 = new Matrix(this.mp, this.mp);
            this.temp4 = new Matrix(this.mp, this.dp);
            this.temp5 = new Matrix(this.mp, 1);
        } else {
            throw new IllegalArgumentException("Kalman filter: Illegal dimensions.");
        }
    }

    public JKalman(int dynam_params, int measure_params) throws Exception {
        this(dynam_params, measure_params, 0);
    }

    public Matrix Predict() {
        return this.Predict((Matrix)null);
    }

    public Matrix Predict(Matrix control) {
        this.state_pre = this.transition_matrix.times(this.state_post);
        if (control != null && this.cp > 0) {
            this.state_pre = this.control_matrix.gemm(control, this.state_pre, 1.0, 1.0);
        }

        this.temp1 = this.transition_matrix.times(this.error_cov_post);
        this.error_cov_pre = this.temp1.gemm(this.transition_matrix.transpose(), this.process_noise_cov, 1.0, 1.0);
        return this.state_pre;
    }

    public Matrix Correct(Matrix measurement) {
        this.temp2 = this.measurement_matrix.times(this.error_cov_pre);
        this.temp3 = this.temp2.gemm(this.measurement_matrix.transpose(), this.measurement_noise_cov, 1.0, 1.0);
        this.temp4 = this.temp3.solve(this.temp2);
        this.gain = this.temp4.transpose();
        this.temp5 = this.measurement_matrix.gemm(this.state_pre, measurement, -1.0, 1.0);
        this.state_post = this.gain.gemm(this.temp5, this.state_pre, 1.0, 1.0);
        this.error_cov_post = this.gain.gemm(this.temp2, this.error_cov_pre, -1.0, 1.0);
        return this.state_post;
    }

    public void setState_pre(Matrix state_pre) {
        this.state_pre = state_pre;
    }

    public Matrix getState_pre() {
        return this.state_pre;
    }

    public void setState_post(Matrix state_post) {
        this.state_post = state_post;
    }

    public Matrix getState_post() {
        return this.state_post;
    }

    public void setTransition_matrix(Matrix transition_matrix) {
        this.transition_matrix = transition_matrix;
    }

    public Matrix getTransition_matrix() {
        return this.transition_matrix;
    }

    public void setControl_matrix(Matrix control_matrix) {
        this.control_matrix = control_matrix;
    }

    public Matrix getControl_matrix() {
        return this.control_matrix;
    }

    public void setMeasurement_matrix(Matrix measurement_matrix) {
        this.measurement_matrix = measurement_matrix;
    }

    public Matrix getMeasurement_matrix() {
        return this.measurement_matrix;
    }

    public void setProcess_noise_cov(Matrix process_noise_cov) {
        this.process_noise_cov = process_noise_cov;
    }

    public Matrix getProcess_noise_cov() {
        return this.process_noise_cov;
    }

    public void setMeasurement_noise_cov(Matrix measurement_noise_cov) {
        this.measurement_noise_cov = measurement_noise_cov;
    }

    public Matrix getMeasurement_noise_cov() {
        return this.measurement_noise_cov;
    }

    public void setError_cov_pre(Matrix error_cov_pre) {
        this.error_cov_pre = error_cov_pre;
    }

    public Matrix getError_cov_pre() {
        return this.error_cov_pre;
    }

    public void setGain(Matrix gain) {
        this.gain = gain;
    }

    public Matrix getGain() {
        return this.gain;
    }

    public void setError_cov_post(Matrix error_cov_post) {
        this.error_cov_post = error_cov_post;
    }

    public Matrix getError_cov_post() {
        return this.error_cov_post;
    }
}
